// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PoseOperations2D.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Vision
{
    using System;
    using System.Collections.Generic;
    using System.Linq;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// 2D operations with poses
    /// </summary>
    public class PoseOperations2D
    {
        /// <summary>
        /// Sort a dictionary of poses by ordering them in the X Y plane in row order
        /// </summary>
        /// <param name="poseDict">Input dictionary of poses</param>
        /// <returns>Sorted list of keys by poses in X Y plane in row order</returns>
        public static List<string> SortPoseDictByRowOrder(Dictionary<string, Pose> poseDict)
        {
            bool first = true;
            double minX = 0.0;
            double maxX = 0.0;
            double[] yVals = new double[poseDict.Count];
            int idx = 0;

            foreach (KeyValuePair<string, Pose> kvp in poseDict)
            {
                if (true == first)
                {
                    minX = kvp.Value.Position.X;
                    maxX = kvp.Value.Position.X;
                    first = false;
                }
                else
                {
                    minX = Math.Min(minX, kvp.Value.Position.X);
                    maxX = Math.Max(maxX, kvp.Value.Position.X);
                }

                yVals[idx++] = kvp.Value.Position.Y;
            }

            double[] yValsSorted = yVals.OrderBy(v => v).ToArray();

            double minY = yValsSorted[0];
            double yDeltaMin = MinDeltaInArray(yValsSorted);
            double width = maxX - minX;

            double ratio = width;
            if (yDeltaMin > 0)
            {
                ratio /= yDeltaMin;
            }

            // Resolves ambiguity for the edge case equality example
            double f = 0.9999;

            var sortedDict = poseDict.OrderBy(k => (k.Value.Position.Y - minY) * ratio + ((k.Value.Position.X - minX) * f));

            List<string> sortedList = sortedDict.Select(kvp => kvp.Key).ToList<string>();

            return sortedList;
        }

        /// <summary>
        /// Checks if the supplied poses lie on a 2D grid
        /// </summary>
        /// <param name="poseDict">Input poses</param>
        /// <param name="sortedList">Sorted index into poses</param>
        /// <param name="countX">Grid count along x direction</param>
        /// <param name="countY">Grid count along Y direction</param>
        /// <param name="fracThresh">
        /// Threshold for setting tolerance when checking positions in grid expressed as
        /// the fraction of the X and Y ranges
        /// </param>
        /// <returns>True if poses lie on a 2D grid</returns>
        public static bool IsPoseListGrid(Dictionary<string, Pose> poseDict, List<string> sortedList, int countX, int countY, double fracThresh = 0.01)
        {
            bool isGrid = true;
            if (poseDict.Count != sortedList.Count ||
                countX * countY != sortedList.Count)
            {
                return false;
            }

            double[] xPositions = new double[countX];
            double[] sortedX = poseDict.Select(k => k.Value.Position.X).OrderBy(v => v).ToArray();
            double xThresh = (sortedX[sortedX.Length - 1] - sortedX[0]) * fracThresh;
            double[] sortedY = poseDict.Select(k => k.Value.Position.Y).OrderBy(v => v).ToArray();
            double yThresh = (sortedY[sortedY.Length - 1] - sortedY[0]) * fracThresh;

            int idx = 0;
            double y = poseDict[sortedList[0]].Position.Y;

            for (int i = 0; i < countX && isGrid == true; ++i)
            {
                Vector3 pos = poseDict[sortedList[idx++]].Position;
                xPositions[i] = pos.X;
                if (Math.Abs(y - pos.Y) > yThresh)
                {
                    isGrid = false;
                }
            }

            for (int j = 1; j < countY && isGrid == true; ++j)
            {
                y = poseDict[sortedList[idx]].Position.Y;
                for (int i = 0; i < countX && isGrid == true; ++i)
                {
                    Vector3 pos = poseDict[sortedList[idx++]].Position;
                    if (Math.Abs(y - pos.Y) > yThresh || Math.Abs(pos.X - xPositions[i]) > xThresh)
                    {
                        isGrid = false;
                    }
                }
            }

            return isGrid;
        }

        /// <summary>
        /// Returns the minimum delta (> 0) between elements
        /// </summary>
        /// <param name="sortedArray">Sorted (ascending) array of values </param>
        /// <returns>Minimum delta</returns>
        private static double MinDeltaInArray(double[] sortedArray)
        {
            if (sortedArray.Length <= 1)
            {
                return 0.0;
            }

            bool isFirst = true;
            double minDelta = 0.0;

            for (int i = 1; i < sortedArray.Length; ++i)
            {
                double delta = sortedArray[i] - sortedArray[i - 1];

                if (delta > 0)
                {
                    if (true == isFirst)
                    {
                        minDelta = delta;
                        isFirst = false;
                    }
                    else
                    {
                        minDelta = Math.Min(minDelta, delta);
                    }
                }
                else if (delta < 0)
                {
                    throw new ArgumentException(string.Format("Array is not sorted ascending between elements {0} and {1}", i - 1, i));
                }
            }

            return minDelta;
        }
    }
}
